/**
 * 放置一些其它文件中用到的共用的函数
 */

#ifndef ZUCMODULE_COMM_H__
#define ZUCMODULE_COMM_H__

#define PY_SSIZE_T_CLEAN
#include <Python.h>
#include "nml_intf/zuc_nml.hh"

static inline PyObject* short_array(short* arr, int sz)
{
    PyObject* res = PyTuple_New(sz);
    for (int i = 0; i < sz; i++) { PyTuple_SET_ITEM(res, i, PyLong_FromLong(arr[i])); }
    return res;
}

static inline PyObject* int_array(int* arr, int sz)
{
    PyObject* res = PyTuple_New(sz);
    for (int i = 0; i < sz; i++) { PyTuple_SET_ITEM(res, i, PyLong_FromLong(arr[i])); }
    return res;
}

static PyObject* float_array(float* arr, int sz)
{
    PyObject* res = PyTuple_New(sz);
    for (int i = 0; i < sz; i++) { PyTuple_SET_ITEM(res, i, PyFloat_FromDouble(arr[i])); }
    return res;
}

static PyObject* double_array(double* arr, int sz)
{
    PyObject* res = PyTuple_New(sz);
    for (int i = 0; i < sz; i++) { PyTuple_SET_ITEM(res, i, PyFloat_FromDouble(arr[i])); }
    return res;
}

static PyObject* double_array(float* arr, int sz)
{
    PyObject* res = PyTuple_New(sz);
    for (int i = 0; i < sz; i++) { PyTuple_SET_ITEM(res, i, PyFloat_FromDouble(arr[i])); }
    return res;
}
static PyObject* zucpose_array(ZucPose pose)
{
    PyObject* res = PyTuple_New(6);
    PyTuple_SET_ITEM(res, 0, PyFloat_FromDouble(pose.tran.x));
    PyTuple_SET_ITEM(res, 1, PyFloat_FromDouble(pose.tran.y));
    PyTuple_SET_ITEM(res, 2, PyFloat_FromDouble(pose.tran.z));
    PyTuple_SET_ITEM(res, 3, PyFloat_FromDouble(pose.a));
    PyTuple_SET_ITEM(res, 4, PyFloat_FromDouble(pose.b));
    PyTuple_SET_ITEM(res, 5, PyFloat_FromDouble(pose.c));
    return res;
}
static inline bool IsBitSet(unsigned int data, int index) { return (data & (1 << index)) != 0; }

//将相对于世界坐标系的笛卡尔坐标转换为相对于当前用户坐标系的笛卡尔坐标
void convertWorldPointToUser(const CartesianPos* base_point, const ZucPose* current_user_offset, CartesianPos* user_point);

//将相对于当前用户坐标系的笛卡尔坐标转换为相对于世界坐标系的笛卡尔坐标
void convertUserPointToWorld(const CartesianPos* user_point, const ZucPose* current_user_offset, CartesianPos* base_point);

void CalculateRotateMatirx(const double* cartPos, double rotate_matrix[3][3]);

void CalculatePayloadCenter(const double sensordata[][CART_DIM], double* MassCenter);

// 传感器辨识负载参数的算法
void CalculatePayloadMass(const double cartpose[][3], const double sensordata[][CART_DIM], double* Mass);

void GetPayLoad(double* PayLoad);

int sem_get(key_t key);
int sem_p(int sem_id);
int sem_v(int sem_id);

#endif  // ZUCMODULE_COMM_H__